import sys
import time
import math
import datetime
import serial
import struct
import copy
import platform
import yaml
import cv2
from PyQt5 import QtCore,QtGui,QtWidgets
import numpy as np

import serctl # 串口控制工具
import imageprocess
import controller

class HJAPP(QtWidgets.QMainWindow): # 主窗口

    close_signal = QtCore.pyqtSignal() # 同步关闭主窗口和子窗口
    
    # 初始化
    def __init__(self):
        super(HJAPP, self).__init__()

        # 读取HSV参数
        self.paramfilepath = 'config/param.yml'
        paramfile = open(self.paramfilepath, 'r', encoding='utf-8')
        paramdata = paramfile.read()
        self.hsvparam = yaml.load(paramdata)

        # 读取Camera参数
        self.camerafilepath = 'config/camera.yml'
        camerafile = open(self.camerafilepath, 'r', encoding='utf-8')
        camerainfo = camerafile.read()
        camera_param = yaml.load(camerainfo)
        self.cam_left_K = np.array(camera_param['Camera Left']['K'])
        self.cam_left_D = np.array(camera_param['Camera Left']['D'])
        self.cam_right_K = np.array(camera_param['Camera Right']['K'])
        self.cam_right_D = np.array(camera_param['Camera Right']['D'])
        self.cam_transform_P = np.array(camera_param['Camera Transform']['P'])
        self.cam_transform_R = np.array(camera_param['Camera Transform']['R'])
        
        # 初始化串口
        self.serialtool = serctl.Serial()

        # 视频变量
        self.image = None
        self.video_frame_rate = 20
        self.video_frame_interval = 50 # 1000/self.video_frame_rate
        self.videoflow = None
        self.videowriter = None
        self.filewriter = None

        # 状态Flag
        self.camera_flag = False
        self.detection_flag = False
        self.recordvideo_flag = False
        self.debugmode_flag = False
        self.control_flag = False
        self.parameter_flag = False
        self.serialport_flag = False
        self.detect_success_flag = False

        # 速度计算
        self.time_interval = 0
        self.pre_time = 0
        self.now_time = time.time()
        self.pre_position = np.zeros((2,))
        self.now_position = np.zeros((2,))
        self.pre_velocity = np.zeros((2,))
        self.now_velocity = np.zeros((2,))
        self.pre_angle = 0
        self.now_angle = 0
        self.pre_angvel = 0
        self.now_angvel = 0

        # 初始化UI
        self.init_ui()
        self.widget_connect()

        # 初始化
        self.controller = controller.AttackAngleController()
        self.close_signal.connect(self.controller.handle_close)
        # self.enable_control_button.clicked.connect(self.controller.handle_click)
       
       
    # 初始化UI界面
    def init_ui(self):
        """
        初始化UI
        :return:
        """
        self.init_layout()
        self.statusBar().showMessage('欢迎使用HJ APP')
        self.setFixedSize(1500, 480)# 设置窗体大小
        self.setWindowTitle('HJ APP')  # 设置窗口标题
        self.show()  # 窗口显示

    # 初始化layout界面
    def init_layout(self):
        """
        初始化UI界面布局
        :return:
        """
        # 定时器
        self.timer = QtCore.QTimer()
        self.timer.setInterval(self.video_frame_interval)
        
        # 布局
        self.main_widget = QtWidgets.QWidget()
        self.main_layout = QtWidgets.QGridLayout()
        self.main_widget.setLayout(self.main_layout)

        # 图片控件
        self.image_window = QtWidgets.QLabel()
        self.image_window.setFixedSize(1280,480)

        # 控制按钮
        self.open_camera_button = QtWidgets.QPushButton('打开相机')
        self.open_camera_button.setFixedSize(90,25)
        self.close_camera_button = QtWidgets.QPushButton('关闭相机')
        self.close_camera_button.setFixedSize(90,25)
        self.enable_detect_button = QtWidgets.QPushButton('开始检测')
        self.enable_detect_button.setFixedSize(90,25)
        self.disable_detect_button = QtWidgets.QPushButton('停止检测')
        self.disable_detect_button.setFixedSize(90,25)
        self.take_photo_button = QtWidgets.QPushButton('拍摄照片')
        self.take_photo_button.setFixedSize(90,25)
        self.record_video_button = QtWidgets.QPushButton('录制视频')
        self.record_video_button.setFixedSize(90,25)
        self.enable_control_button = QtWidgets.QPushButton('开启控制')
        self.enable_control_button.setFixedSize(90,25)
        self.disable_control_button = QtWidgets.QPushButton('关闭控制')
        self.disable_control_button.setFixedSize(90,25)
        # 参数调节
        self.param_adjust_label = QtWidgets.QLabel('参数调节')
        self.param_adjust_label.setFont(QtGui.QFont('Microsoft YaHei', 10, QtGui.QFont.Bold))
        self.red_marker_checkbox = QtWidgets.QCheckBox("红色标记")
        self.yellow_marker_checkbox = QtWidgets.QCheckBox("黄色标记")
        self.red_marker_checkbox.setChecked(True)

        self.hue_low_label = QtWidgets.QLabel('Hmin')
        self.hue_low_splider = QtWidgets.QSlider(QtCore.Qt.Horizontal)
        self.hue_high_label = QtWidgets.QLabel('Hmax')
        self.hue_high_splider = QtWidgets.QSlider(QtCore.Qt.Horizontal)
        self.satur_low_label = QtWidgets.QLabel('Smin')
        self.satur_low_splider = QtWidgets.QSlider(QtCore.Qt.Horizontal)
        self.satur_high_label = QtWidgets.QLabel('Smax')
        self.satur_high_splider = QtWidgets.QSlider(QtCore.Qt.Horizontal)
        self.value_low_label = QtWidgets.QLabel('Vmin')
        self.value_low_splider = QtWidgets.QSlider(QtCore.Qt.Horizontal)
        self.value_high_label = QtWidgets.QLabel('Vmax')
        self.value_high_splider = QtWidgets.QSlider(QtCore.Qt.Horizontal)
        self.hue_low_splider.setMinimum(0)
        self.hue_low_splider.setMaximum(255)
        self.hue_high_splider.setMinimum(0)
        self.hue_high_splider.setMaximum(255)
        self.satur_low_splider.setMinimum(0)
        self.satur_low_splider.setMaximum(255)
        self.satur_high_splider.setMinimum(0)
        self.satur_high_splider.setMaximum(255)
        self.value_low_splider.setMinimum(0)
        self.value_low_splider.setMaximum(255)
        self.value_high_splider.setMinimum(0)
        self.value_high_splider.setMaximum(255)

        self.refresh_hsvparam_ui()

        self.debug_mode_button = QtWidgets.QPushButton('调试模式')
        self.debug_mode_button.setFixedSize(90,25)
        self.save_parameter_button = QtWidgets.QPushButton('保存参数')
        self.save_parameter_button.setFixedSize(90,25)
        # 串口控制
        self.serial_control_label = QtWidgets.QLabel('串口控制')
        self.serial_control_label.setFont(QtGui.QFont('Microsoft YaHei', 10, QtGui.QFont.Bold))
        self.serial_com_label = QtWidgets.QLabel('COM')
        self.serial_com_combo = QtWidgets.QComboBox()
        self.serial_com_combo.addItem('COM3')
        self.serial_com_combo.addItem('COM5')
        self.serial_com_combo.addItem('COM6')
        self.serial_com_combo.addItem('COM7')
        self.serial_com_combo.addItem('COM9')
        self.serial_com_combo.addItem('COM10')
        self.serial_com_combo.addItem('COM11')
        self.serial_com_combo.addItem('COM12')
        self.serial_com_combo.addItem('COM13')
        self.serial_com_combo.addItem('COM28')

        self.serial_bps_label = QtWidgets.QLabel('BPS')
        self.serial_bps_combo = QtWidgets.QComboBox()
        self.serial_bps_combo.addItem('9600')
        self.serial_bps_combo.addItem('28400')
        self.serial_bps_combo.addItem('19200')
        self.serial_bps_combo.addItem('38400')
        self.serial_bps_combo.addItem('56000')
        self.serial_bps_combo.addItem('57600')
        self.serial_bps_combo.addItem('129200')

        self.serial_open_button = QtWidgets.QPushButton('开始发送')
        self.serial_open_button.setFixedSize(90,25)
        self.serial_close_button = QtWidgets.QPushButton('停止发送')
        self.serial_close_button.setFixedSize(90,25)

        # 布局,29行17列
        self.main_layout.addWidget(self.image_window, 0, 0, 14, 28)

        self.main_layout.addWidget(self.open_camera_button, 0, 28, 1, 2)
        self.main_layout.addWidget(self.close_camera_button, 0, 30, 1, 2)
        self.main_layout.addWidget(self.enable_detect_button, 1, 28, 1, 2)
        self.main_layout.addWidget(self.disable_detect_button, 1, 30, 1, 2)
        self.main_layout.addWidget(self.take_photo_button, 2, 28, 1, 2)
        self.main_layout.addWidget(self.record_video_button, 2, 30, 1, 2)
        self.main_layout.addWidget(self.enable_control_button, 3, 28, 1, 2)
        self.main_layout.addWidget(self.disable_control_button, 3, 30, 1, 2)
        
        self.main_layout.addWidget(self.param_adjust_label, 4, 28, 1, 4)
        self.main_layout.addWidget(self.red_marker_checkbox, 5, 28, 1, 2)
        self.main_layout.addWidget(self.yellow_marker_checkbox, 5, 30, 1, 2)
        self.main_layout.addWidget(self.hue_low_label, 6, 28, 1, 1)
        self.main_layout.addWidget(self.hue_low_splider, 6, 29, 1, 3)
        self.main_layout.addWidget(self.hue_high_label, 7, 28, 1, 1)
        self.main_layout.addWidget(self.hue_high_splider, 7, 29, 1, 3)
        self.main_layout.addWidget(self.satur_low_label, 8, 28, 1, 1)
        self.main_layout.addWidget(self.satur_low_splider, 8, 29, 1, 3)
        self.main_layout.addWidget(self.satur_high_label, 9, 28, 1, 1)
        self.main_layout.addWidget(self.satur_high_splider, 9, 29, 1, 3)
        self.main_layout.addWidget(self.value_low_label, 10, 28, 1, 1)
        self.main_layout.addWidget(self.value_low_splider, 10, 29, 1, 3)
        self.main_layout.addWidget(self.value_high_label, 11, 28, 1, 1)
        self.main_layout.addWidget(self.value_high_splider, 11, 29, 1, 3)
        self.main_layout.addWidget(self.debug_mode_button, 12, 28, 1, 2)
        self.main_layout.addWidget(self.save_parameter_button, 12, 30, 1, 2)

        self.main_layout.addWidget(self.serial_control_label, 13, 28, 1, 4)
        self.main_layout.addWidget(self.serial_com_label, 14, 28, 1, 1)
        self.main_layout.addWidget(self.serial_com_combo, 14, 29, 1, 3)
        self.main_layout.addWidget(self.serial_bps_label, 15, 28, 1, 1)
        self.main_layout.addWidget(self.serial_bps_combo, 15, 29, 1, 3)
        self.main_layout.addWidget(self.serial_open_button, 16, 28, 1, 2)
        self.main_layout.addWidget(self.serial_close_button, 16, 30, 1, 2)
        
        self.setCentralWidget(self.main_widget)  # 设置窗口主部件

    def widget_connect(self):
        self.timer.timeout.connect(self.play_video)
        self.open_camera_button.clicked.connect(self.open_camera_button_slot)
        self.close_camera_button.clicked.connect(self.close_camera_button_slot)
        self.enable_detect_button.clicked.connect(self.enable_detect_button_slot)
        self.disable_detect_button.clicked.connect(self.disable_detect_button_slot)
        self.serial_open_button.clicked.connect(self.serial_open_button_slot)
        self.serial_close_button.clicked.connect(self.serial_close_button_slot)
        self.yellow_marker_checkbox.stateChanged.connect(self.yellow_marker_checkbox_slot)
        self.red_marker_checkbox.stateChanged.connect(self.red_marker_checkbox_slot)
        self.debug_mode_button.clicked.connect(self.debug_mode_button_slot)
        self.save_parameter_button.clicked.connect(self.save_parameter_button_slot)
        self.take_photo_button.clicked.connect(self.take_photo_button_slot)
        self.record_video_button.clicked.connect(self.record_video_button_slot)
        self.enable_control_button.clicked.connect(self.enable_control_button_slot)
        self.disable_control_button.clicked.connect(self.disable_control_button_slot)


    def play_video(self):
        if (self.videoflow.isOpened()):
            # 获取图像
            ret, img = self.videoflow.read()
            height, width, bytesPerComponent = img.shape
            bytesPerLine = bytesPerComponent * width

            #imageprocess.undistort_image(img[:,640:1280,:], self.cam_left_K, self.cam_left_D)
            u1 = 0
            u2 = 0
            if self.detection_flag:
                img = self.detection(img)
                if self.control_flag and self.detect_success_flag:
                    # 全局坐标系下的角度、速度，转换至体坐标系
                    angle = -self.now_angle
                    angvel = -self.now_angvel
                    velX = self.now_velocity[0] * np.cos(angle) - self.now_velocity[1] * np.sin(angle)
                    velZ = self.now_velocity[0] * np.sin(angle) + self.now_velocity[1] * np.cos(angle)
                    if abs(velX) > 0.01 or abs(velZ) > 0.01:
                        u1, u2 = self.controller.control(self.now_time, self.time_interval, angle, angvel, velX, velZ)
                    if self.serialport_flag and u1 != 0 and u2 != 0:
                        byte1 = (int.from_bytes(b'\x11\x50', byteorder='big',signed=False) + u1).to_bytes(length=2, byteorder='big', signed=False)
                        if u2 > 0:
                            byte2 = (int.from_bytes(b'\x14\x00', byteorder='big',signed=False) + abs(u2)).to_bytes(length=2, byteorder='big', signed=False)
                        else:
                            byte2 = (int.from_bytes(b'\x17\x00', byteorder='big',signed=False) + abs(u2)).to_bytes(length=2, byteorder='big', signed=False)
                        databyte = byte1 + byte2
                        self.serialtool.write_cmd(databyte)


            if self.recordvideo_flag:
                self.videowriter.write(img)
                datastr = ('%.2f' % self.now_time) + ' ' + ('%.6f' % self.now_angle) + ' ' + ('%.6f' % self.now_angvel) + ' ' + ('%.6f' % self.now_velocity[0]) + ' ' + ('%.6f' % self.now_velocity[0]) + \
                      ' ' + ('%.6f' % self.now_position[0]) + ' ' + ('%.6f' % self.now_position[1]) + ' ' + ('%d' % u1) + ' ' + ('%d' % u2) + '\n'
                self.filewriter.write(datastr)

            
            # 保存当前帧
            self.image = img

            # 变换彩色空间顺序
            rgbimg = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)

            # 转为QImage对象
            qtimage = QtGui.QImage(rgbimg.data.tobytes(), width, height, bytesPerLine, QtGui.QImage.Format_RGB888)
            self.image_window.setPixmap(QtGui.QPixmap.fromImage(qtimage).scaled(self.image_window.width(), self.image_window.height()))


    def detection(self, img):
        blur = cv2.GaussianBlur(img, (7, 7), 0)
        hsv = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV)
        if self.debugmode_flag:
            low_hue = self.hue_low_splider.value()
            high_hue = self.hue_high_splider.value()
            low_sat = self.satur_low_splider.value()
            high_sat = self.satur_high_splider.value()
            low_val = self.value_low_splider.value()
            high_val = self.value_high_splider.value()
            color_low = np.array([low_hue, low_sat, low_val])
            color_high = np.array([high_hue, high_sat, high_val])
            thres = cv2.inRange(hsv, color_low, color_high)
            kernal = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (7, 7))
            mask = cv2.morphologyEx(thres, cv2.MORPH_CLOSE, kernal)
            mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernal)
            result = cv2.bitwise_and(img, img, mask = mask)
            return result
        
        # 检测红色标记
        red_low_hue = self.hsvparam['RedTarget']['Hmin']
        red_high_hue = self.hsvparam['RedTarget']['Hmax']
        red_low_sat = self.hsvparam['RedTarget']['Smin']
        red_high_sat = self.hsvparam['RedTarget']['Smax']
        red_low_val = self.hsvparam['RedTarget']['Vmin']
        red_high_val = self.hsvparam['RedTarget']['Vmax']
        color_low = np.array([red_low_hue, red_low_sat, red_low_val])
        color_high = np.array([red_high_hue, red_high_sat, red_high_val])
        red_ellipse_left, red_ellipse_right = imageprocess.detect_ellipse(hsv, color_low, color_high)

        # 检测黄色标记
        yellow_low_hue = self.hsvparam['YellowTarget']['Hmin']
        yellow_high_hue = self.hsvparam['YellowTarget']['Hmax']
        yellow_low_sat = self.hsvparam['YellowTarget']['Smin']
        yellow_high_sat = self.hsvparam['YellowTarget']['Smax']
        yellow_low_val = self.hsvparam['YellowTarget']['Vmin']
        yellow_high_val = self.hsvparam['YellowTarget']['Vmax']
        color_low = np.array([yellow_low_hue, yellow_low_sat, yellow_low_val])
        color_high = np.array([yellow_high_hue, yellow_high_sat, yellow_high_val])
        yellow_ellipse_left, yellow_ellipse_right = imageprocess.detect_ellipse(hsv, color_low, color_high)

        red_point_pos = None
        yellow_point_pos = None
        if red_ellipse_left is not None and yellow_ellipse_left is not None:

            red_left_point = np.array(red_ellipse_left[0])
            red_left_point = red_left_point.reshape(1, 1, 2) # 点必须是三维的（1,1,2)，第一维是点的个数
            red_right_point = np.array(red_ellipse_right[0])
            red_right_point = red_right_point.reshape(1, 1, 2)
            red_right_point[0,0,0] = red_right_point[0,0,0] - 640
            red_point_pos = imageprocess.triangulation(red_left_point, red_right_point, self.cam_left_K, self.cam_left_D,
                                                    self.cam_right_K, self.cam_right_D, self.cam_transform_P, self.cam_transform_R)
            
            yellow_left_point = np.array(yellow_ellipse_left[0])
            yellow_left_point = yellow_left_point.reshape(1, 1, 2) # 点必须是三维的（1,1,2)，第一维是点的个数
            yellow_right_point = np.array(yellow_ellipse_right[0])
            yellow_right_point = yellow_right_point.reshape(1, 1, 2)
            yellow_right_point[0,0,0] = yellow_right_point[0,0,0] - 640
            yellow_point_pos = imageprocess.triangulation(yellow_left_point, yellow_right_point, self.cam_left_K, self.cam_left_D,
                                                    self.cam_right_K, self.cam_right_D, self.cam_transform_P, self.cam_transform_R)
            
        # 计算速度
        if red_point_pos is not None and yellow_point_pos is not None:
            if self.pre_time == 0:
                self.pre_time = time.time()
                self.now_time = time.time()
                self.pre_position = (red_point_pos[:2] + yellow_point_pos[:2])/2/1000 # 单位：米
                self.now_position = self.pre_position
                self.pre_velocity = np.zeros((2,))
                self.now_velocity = self.pre_velocity

                vec_r2y = yellow_point_pos[:2] - red_point_pos[:2]
                self.pre_angle = math.atan2(vec_r2y[1], vec_r2y[0]) # 单位：弧度
                self.pre_angle = self.pre_angle
                self.pre_angvel = 0
                self.now_angvel = self.pre_angvel
            else:
                self.pre_time = self.now_time
                self.now_time = time.time()
                time_interval = self.now_time - self.pre_time # 单位：秒
                self.time_interval = time_interval

                alpha = 0.8
                beta = 1 - alpha
                self.now_position = (red_point_pos[:2] + yellow_point_pos[:2])/2/1000 # 单位：米
                self.now_velocity = (self.now_position - self.pre_position) / time_interval
                self.now_velocity = self.pre_velocity * alpha + self.now_velocity * beta
                self.pre_position = self.now_position
                self.pre_velocity = self.now_velocity

                vec_r2y = yellow_point_pos[:2] - red_point_pos[:2]
                self.now_angle = math.atan2(vec_r2y[1], vec_r2y[0])
                self.now_angvel = (self.now_angle - self.pre_angle) / time_interval
                self.now_angvel = self.pre_angvel * alpha + self.now_angvel *beta
                self.pre_angle = self.now_angle
                self.pre_angvel = self.now_angvel
            self.detect_success_flag = True
        else:
            self.detect_success_flag = False

                
        # 绘图
        if red_ellipse_left is not None:
            ellipse_center = red_ellipse_left[0]
            x = round(ellipse_center[0])
            y = round(ellipse_center[1])
            img = cv2.ellipse(img, red_ellipse_left, (0,0,255), 2)
            cv2.putText(img, "red left", (x, y - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
            img = cv2.circle(img, (x, y), 2, (0,0,255), 2)
            if red_point_pos is not None:
                pos_str = "pos:[" + str(int(red_point_pos[0])) + ", " + str(int(red_point_pos[1])) + ", " + str(int(red_point_pos[2])) + "]"
                cv2.putText(img, pos_str, (x, y + 20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 2)
                
            
            ellipse_center = red_ellipse_right[0]
            x = round(ellipse_center[0])
            y = round(ellipse_center[1])
            img = cv2.ellipse(img, red_ellipse_right, (0,0,255), 2)
            cv2.putText(img, "red right", (x, y - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
            img = cv2.circle(img, (x, y), 2, (0,0,255), 2)
            
        if yellow_ellipse_left is not None:
            ellipse_center = yellow_ellipse_left[0]
            x = round(ellipse_center[0])
            y = round(ellipse_center[1])
            img = cv2.ellipse(img, yellow_ellipse_left, (0,255,255), 2)
            cv2.putText(img, "yellow left", (x, y - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
            img = cv2.circle(img, (x, y), 2, (0,255,255), 2)
            if yellow_point_pos is not None:
                pos_str = "pos:[" + str(int(yellow_point_pos[0])) + ", " + str(int(yellow_point_pos[1])) + ", " + str(int(yellow_point_pos[2])) + "]"
                cv2.putText(img, pos_str, (x, y + 20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 2)

            ellipse_center = yellow_ellipse_right[0]
            x = round(ellipse_center[0])
            y = round(ellipse_center[1])
            img = cv2.ellipse(img, yellow_ellipse_right, (0,255,255), 2)
            cv2.putText(img, "yellow right", (x, y - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
            img = cv2.circle(img, (x, y), 2, (0,255,255), 2)

        if self.now_velocity is not None and self.now_angvel is not None:
            vel_x_str = "Velocity X: " + ('%.2f' % self.now_velocity[0])
            vel_y_str = "Velocity Y: " + ('%.2f' % self.now_velocity[1]) 
            ang_str = "Angular vel: " + ('%.2f' % (self.now_angvel/math.pi*180))
            cv2.putText(img, vel_x_str, (110, 25), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
            cv2.putText(img, vel_y_str, (110, 50), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
            cv2.putText(img, ang_str, (110, 75), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
            # 坐标系
            origin_point = (25,15)
            x_point = (75, 15)
            y_point = (25, 65)
            img = cv2.line(img, origin_point, x_point, (255, 255, 255), 2, 4)
            img = cv2.line(img, origin_point, y_point, (255, 255, 255), 2, 4)
            img = cv2.circle(img, origin_point, 2, (255,255,255), 2)
            img = cv2.circle(img, x_point, 2, (255,255,255), 2)
            img = cv2.circle(img, y_point, 2, (255,255,255), 2)
            cv2.putText(img, 'x', (85, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
            cv2.putText(img, 'y', (20, 85), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)

        result = img
        return result

    def open_camera_button_slot(self):
        self.videoflow = cv2.VideoCapture(0, cv2.CAP_DSHOW)
        self.videoflow.set(3, 1280) # set width: CV_CAP_PROP_FRAME_WIDTH 
        self.videoflow.set(4, 480) # set height: CV_CAP_PROP_FRAME_HEIGHT 
        
        self.timer.start()
        if (self.videoflow.isOpened()):
            self.camera_flag = True
        st = self.statusbar_text()
        self.statusBar().showMessage(st)
    

    def close_camera_button_slot(self):
        try:
            self.timer.stop()
            self.videoflow.release()
            self.camera_flag = False
            self.detection_flag = False
            self.velocity_flag = False
            self.angulvel_flag = False
            self.control_flag = False
            self.parameter_flag = False
            st = self.statusbar_text()
            self.statusBar().showMessage(st)
        except AttributeError:
            pass
    
    def enable_detect_button_slot(self):
        self.detection_flag = True
        st = self.statusbar_text()
        self.statusBar().showMessage(st)
    
    def disable_detect_button_slot(self):
        self.detection_flag = False
        st = self.statusbar_text()
        self.statusBar().showMessage(st)

    def yellow_marker_checkbox_slot(self):
        if self.yellow_marker_checkbox.isChecked():
            self.red_marker_checkbox.setChecked(False)
        self.refresh_hsvparam_ui()

    def red_marker_checkbox_slot(self):
        if self.red_marker_checkbox.isChecked():
            self.yellow_marker_checkbox.setChecked(False)
        self.refresh_hsvparam_ui()

    def serial_open_button_slot(self):
        """
        串口打开按钮对应的槽函数
        :return:
        """
        port = self.serial_com_combo.currentText()
        baud = int(self.serial_bps_combo.currentText())
        try:
            self.serialtool.init_serial(port,baud)
            self.serialport_flag = True
            st = self.statusbar_text()
            self.statusBar().showMessage(st)
        except serial.serialutil.SerialException:
            self.serialport_flag = False
            self.statusBar().showMessage('串口不存在')


    def serial_close_button_slot(self):
        """
        串口关闭对应的槽函数
        :return:
        """
        self.serialtool.close_serial()
        self.serialport_flag = False
        st = self.statusbar_text()
        self.statusBar().showMessage(st)

    def debug_mode_button_slot(self):
        self.debugmode_flag = not self.debugmode_flag
        st = self.statusbar_text()
        self.statusBar().showMessage(st)

    def save_parameter_button_slot(self):
        itemstr = 'Others'
        if self.yellow_marker_checkbox.isChecked():
            itemstr = 'YellowTarget'
        elif self.red_marker_checkbox.isChecked():
            itemstr = 'RedTarget'
        else:
            itemstr = 'Others'
        self.hsvparam[itemstr]['Hmin'] = self.hue_low_splider.value()
        self.hsvparam[itemstr]['Hmax'] = self.hue_high_splider.value()
        self.hsvparam[itemstr]['Smin'] = self.satur_low_splider.value()
        self.hsvparam[itemstr]['Smax'] = self.satur_high_splider.value()
        self.hsvparam[itemstr]['Vmin'] = self.value_low_splider.value()
        self.hsvparam[itemstr]['Vmax'] = self.value_high_splider.value()
        with open(self.paramfilepath, "w", encoding="utf-8") as f:
            yaml.dump(self.hsvparam, f)

    def take_photo_button_slot(self):
        if self.image is not None:
            now_time = datetime.datetime.now()
            imagename = 'image/' + str(now_time.year-2000).zfill(2) + str(now_time.month).zfill(2) + str(now_time.day).zfill(2) + \
                        str(now_time.hour).zfill(2) + str(now_time.minute).zfill(2)+ str(now_time.second).zfill(2) + '_left.jpg'
            cv2.imwrite(imagename, self.image[:,0:640,:])
            imagename = 'image/' + str(now_time.year-2000).zfill(2) + str(now_time.month).zfill(2) + str(now_time.day).zfill(2) + \
                        str(now_time.hour).zfill(2) + str(now_time.minute).zfill(2)+ str(now_time.second).zfill(2) + '_right.jpg'
            cv2.imwrite(imagename, self.image[:,640:1280,:])
        st = self.statusbar_text()
        self.statusBar().showMessage(st)

    def record_video_button_slot(self):
        self.recordvideo_flag = not self.recordvideo_flag
        if self.videoflow.isOpened() and self.recordvideo_flag:
            self.record_video_button.setText('停止录制')
            now_time = datetime.datetime.now()
            videoname = 'video/' + str(now_time.year-2000).zfill(2) + str(now_time.month).zfill(2) + str(now_time.day).zfill(2) + \
                        str(now_time.hour).zfill(2) + str(now_time.minute).zfill(2) + str(now_time.second).zfill(2) + '.avi'
            fourcc = cv2.VideoWriter_fourcc(*'XVID')
            self.videowriter = cv2.VideoWriter(videoname, fourcc, self.video_frame_rate, (1280, 480))
            filename = 'video/' + str(now_time.year-2000).zfill(2) + str(now_time.month).zfill(2) + str(now_time.day).zfill(2) + \
                        str(now_time.hour).zfill(2) + str(now_time.minute).zfill(2) + str(now_time.second).zfill(2) + '.txt'
            self.filewriter = open(filename, 'w')
        else:
            self.recordvideo_flag = False
            self.record_video_button.setText('录制视频')
            self.videowriter.release()
            self.filewriter.close()
        st = self.statusbar_text()
        self.statusBar().showMessage(st)

    def enable_control_button_slot(self):
        self.control_flag = True
        self.controller.handle_click()
        st = self.statusbar_text()
        self.statusBar().showMessage(st)
    
    def disable_control_button_slot(self):
        self.control_flag = False
        self.controller.handle_close()
        st = self.statusbar_text()
        self.statusBar().showMessage(st)

    def refresh_hsvparam_ui(self):
        itemstr = 'Others'
        if self.yellow_marker_checkbox.isChecked():
            itemstr = 'YellowTarget'
        elif self.red_marker_checkbox.isChecked():
            itemstr = 'RedTarget'
        else:
            itemstr = 'Others'
        self.hue_low_splider.setValue(self.hsvparam[itemstr]['Hmin'])
        self.hue_high_splider.setValue(self.hsvparam[itemstr]['Hmax'])
        self.satur_low_splider.setValue(self.hsvparam[itemstr]['Smin'])
        self.satur_high_splider.setValue(self.hsvparam[itemstr]['Smax'])
        self.value_low_splider.setValue(self.hsvparam[itemstr]['Vmin'])
        self.value_high_splider.setValue(self.hsvparam[itemstr]['Vmax'])

    def statusbar_text(self):
        statustext = ''
        if self.camera_flag:
            statustext = statustext + '相机已打开，'
            if self.detection_flag:
                if self.control_flag:
                    statustext = statustext + '控制已启用，'
                else:
                    statustext = statustext + '控制未启用，'
            else:
                statustext = statustext + '检测未启用，'
        else:
            statustext = statustext + '相机未打开, '

        if self.serialport_flag:
            statustext = statustext + '串口已启用, '
        else:
            statustext = statustext + '串口未启用, '

        if self.debugmode_flag:
            statustext = statustext + '调试模式'
        else:
            statustext = statustext + '正常模式'

        return statustext


    def closeEvent(self, event):
        self.close_signal.emit()
        self.close()



if __name__ == '__main__':

    app = QtWidgets.QApplication(sys.argv)  # 创建QApplication对象是必须，管理整个程序，参数可有可无，有的话可接收命令行参数

    hjapp = HJAPP()  # 创建窗体对象
    
    sys.exit(app.exec_())
